clc;
clear all;
close all;

%% Parameters
Rm= 2.6;
Kt=0.00767;
Km=0.00767;
Kg=3.71;
nm=1;
ng=1;
Mc=0.52;
Msw=3.6;
Jsw=0.3950;
Dt=0.1250;
Dc=0.058;
r=0.00635;
g=9.81;
Beq=0.9;
Bsw=0;

%% Coefficienten

a31= -Mc*Dt*g/Jsw;
a32=(-g*Mc*Rm*r^2*Jsw+Mc*Dt*Rm*r^2*g*Msw*Dc)/(Rm*r^2*Jsw*Mc);
a33=(-Jsw*ng*Kg^2*nm*Kt*Km-Jsw*Beq*Rm*r^2-Mc*Dt^2*ng*Kg^2*nm*Kt*Km-Mc*Dt^2*Beq*Rm*r^2)/(Rm*r^2*Jsw*Mc);
a34=-Dt*Bsw/Jsw;
a41=-g*Mc/Jsw;
a42=g*Msw*Dc/Jsw;
a43=(-ng*Kg^2*nm*Kt*Km*Dt-Beq*Rm*r^2*Dt)/(Rm*r^2*Jsw);
a44=-Bsw/Jsw;
b3=(Jsw*ng*Kg*nm*Kt*r+Mc*Dt^2*ng*Kg*nm*Kt*r)/(Rm*r^2*Jsw*Mc);
b4=(ng*Kg*nm*Kt*Dt)/(r*Rm*Jsw);

A=[0 0 1 0; 0 0 0 1; a31 a32 a33 a34; a41 a42 a43 a44];
B=[0;0;b3;b4];

%% Discretisatie

Cad = [4.41/0.456 0 0 0; 0 3.166/(15/180*pi) 0 0];
 Dad=[1/Cad(1,1) 0; 0 1/Cad(2,2)];
wc=2*2*pi;
Ts=1/200;

%% Controller

Q=diag([1000 4000 0 0]);
R=10;
[K,S,e]=lqr(A,B,Q,R);